#include "mk20dx128.h"
#include "core_pins.h"
#include "HardwareSerial.h"

// UART0 and UART1 are clocked by F_CPU, UART2 is clocked by F_BUS
// UART0 has 8 byte fifo, UART1 and UART2 have 1 byte buffer

#define TX_BUFFER_SIZE 40
static volatile uint8_t tx_buffer[TX_BUFFER_SIZE];
static volatile uint8_t tx_buffer_head = 0;
static volatile uint8_t tx_buffer_tail = 0;
static volatile uint8_t transmitting = 0;

#define RX_BUFFER_SIZE 64
static volatile uint8_t rx_buffer[RX_BUFFER_SIZE];
static volatile uint8_t rx_buffer_head = 0;
static volatile uint8_t rx_buffer_tail = 0;

#define C2_ENABLE		UART_C2_TE | UART_C2_RE | UART_C2_RIE
#define C2_TX_ACTIVE		C2_ENABLE | UART_C2_TIE
#define C2_TX_COMPLETING	C2_ENABLE | UART_C2_TCIE
#define C2_TX_INACTIVE		C2_ENABLE

void serial3_begin(uint32_t divisor)
{
	SIM_SCGC4 |= SIM_SCGC4_UART2;	// turn on clock, TODO: use bitband
	rx_buffer_head = 0;
	rx_buffer_tail = 0;
	tx_buffer_head = 0;
	tx_buffer_tail = 0;
	transmitting = 0;
	CORE_PIN7_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_PFE | PORT_PCR_MUX(3);
	CORE_PIN8_CONFIG = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(3);
	UART2_BDH = (divisor >> 13) & 0x1F;
	UART2_BDL = (divisor >> 5) & 0xFF;
	UART2_C4 = divisor & 0x1F;
	UART2_C1 = 0;
	UART2_PFIFO = 0;
	UART2_C2 = C2_TX_INACTIVE;
	NVIC_ENABLE_IRQ(IRQ_UART2_STATUS);
}

void serial3_end(void)
{
	if (!(SIM_SCGC4 & SIM_SCGC4_UART2)) return;
	while (transmitting) yield();  // wait for buffered data to send
	NVIC_DISABLE_IRQ(IRQ_UART2_STATUS);
	UART2_C2 = 0;
	CORE_PIN7_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
	CORE_PIN8_CONFIG = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
	rx_buffer_head = 0;
	rx_buffer_tail = 0;
}

void serial3_putchar(uint8_t c)
{
	uint32_t head;

	if (!(SIM_SCGC4 & SIM_SCGC4_UART2)) return;
	head = tx_buffer_head;
	if (++head >= TX_BUFFER_SIZE) head = 0;
	while (tx_buffer_tail == head) {
		yield(); // wait
	}
	tx_buffer[head] = c;
	transmitting = 1;
	tx_buffer_head = head;
	UART2_C2 = C2_TX_ACTIVE;
}

void serial3_flush(void)
{
	while (transmitting) yield(); // wait
}

int serial3_available(void)
{
	uint8_t head, tail;

	head = rx_buffer_head;
	tail = rx_buffer_tail;
	if (head >= tail) return head - tail;
	return RX_BUFFER_SIZE + head - tail;
}

int serial3_getchar(void)
{
	uint8_t head, tail;
	int c;

	head = rx_buffer_head;
	tail = rx_buffer_tail;
	if (head == tail) return -1;
	if (++tail >= RX_BUFFER_SIZE) tail = 0;
	c = rx_buffer[tail];
	rx_buffer_tail = tail;
	return c;
}

int serial3_peek(void)
{
	uint8_t head, tail;

	head = rx_buffer_head;
	tail = rx_buffer_tail;
	if (head == tail) return -1;
	return rx_buffer[tail];
}

void serial3_clear(void)
{
	rx_buffer_head = rx_buffer_tail;
}

// status interrupt combines 
//   Transmit data below watermark  UART_S1_TDRE
//   Transmit complete              UART_S1_TC
//   Idle line                      UART_S1_IDLE
//   Receive data above watermark   UART_S1_RDRF
//   LIN break detect               UART_S2_LBKDIF
//   RxD pin active edge            UART_S2_RXEDGIF

void uart2_status_isr(void)
{
	uint8_t avail, head, newhead, tail, c;

	//digitalWriteFast(4, HIGH);
	if (UART2_S1 & UART_S1_RDRF) {
		//digitalWriteFast(5, HIGH);
		c = UART2_D;
		head = rx_buffer_head + 1;
		if (head >= RX_BUFFER_SIZE) head = 0;
		if (head != rx_buffer_tail) {
			rx_buffer[head] = c;
			rx_buffer_head = head; 
		}
		//digitalWriteFast(5, LOW);
	}
	c = UART2_C2;
	if ((c & UART_C2_TIE) && (UART2_S1 & UART_S1_TDRE)) {
		//digitalWriteFast(5, HIGH);
		head = tx_buffer_head;
		tail = tx_buffer_tail;
		if (head == tail) {
			UART2_C2 = C2_TX_COMPLETING;
		} else {
			if (++tail >= TX_BUFFER_SIZE) tail = 0;
			UART2_D = tx_buffer[tail];
			tx_buffer_tail = tail;
		}
		//digitalWriteFast(5, LOW);
	}
	if ((c & UART_C2_TCIE) && (UART2_S1 & UART_S1_TC)) {
		transmitting = 0;
		UART2_C2 = C2_TX_INACTIVE;
	}
	//digitalWriteFast(4, LOW);
}


